﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using Optimization.Utilities;

namespace Optimization.StandardSolvers
{

    class Planet : Particle
    {
        public double Mass;
        public Vector Force = null;
    }

    [IsSolver]
    public class GSA : ParticleBasedMethod, ISolver
    {

        static public int DefaultUniversumSize = 50;
        static public float DefaultSpeedDown = 0.9f;
        static public float DefaultStepSize = 0.1f;

        System.Random rnd = new Optimization.Utilities.Random();

        double G = 100;
        int iteration = 0;
        double externalSolutionMass = 0;
        int swarmSize = 50;

        void UpdateBestSolution()
        {
            Planet bestParticle = null;
            foreach (Planet p in this)
                if (p.Value < SolutionValue)
                {
                    bestParticle = p;
                    SolutionValue = p.Value;
                    Solution = new Vector(p.Position);
                }

            //put best on first place
            if (bestParticle != null)
            {
                this.Remove(bestParticle);
                this.Insert(0, bestParticle);
            }

        }

        public void DefineProblem(Func<IProblem> cf)
        {
            ProblemFactory = cf;
            Problem = ProblemFactory();

            Solution = Problem.RandomSolution();
            SolutionValue = Problem.Evaluate(Solution);

            Init();
        }

        public Vector GetSolution()
        {
            return Solution.Copy();
        }

        public void SetSolution(Vector solution, double solutionValue)
        {
            if (this.SolutionValue > solutionValue)
            {
                //find the worste planet
                Planet maximum = this[0] as Planet;
                foreach (Planet p in this)
                    if (p.Value > maximum.Value)
                        maximum = p;

                //update it to injected solution
                maximum.Position = solution.Copy();
                maximum.Value = solutionValue;
                maximum.Velocity = Vector.Zero(maximum.Position.Length);
                maximum.Force = maximum.Velocity.Copy();
            }
        }

        public double GetSolutionValue()
        {
            return SolutionValue;
        }

        void Init()
        {
            this.Clear();
            for (int i = 0; i < swarmSize; i++)
            {
                Planet p = new Planet();
                p.Position = Problem.RandomSolution();
                p.Value = Problem.Evaluate(p.Position);
                p.Velocity = Vector.Zero(p.Position.Length);
                this.Add(p);
            }
            iteration = 0;
        }

        public void Step()
        {
            //compute masses
            double minVal = double.MaxValue;
            double maxVal = -double.MaxValue;

            foreach (Planet p in this)
            {
                if (p.Value < minVal)
                    minVal = p.Value;
                if (p.Value > maxVal)
                    maxVal = p.Value;
            }

            if (externalSolution != null)
            {
                if (externalSolutionValue < minVal)
                    minVal = externalSolutionValue;
                if (externalSolutionValue > maxVal)
                    maxVal = externalSolutionValue;
            }

            //m(i)=(fit(i) - minVal) / (maxVal - minVal); = fit(i) / deltaMinMax - minVal / deltaMinMax;
            double deltaMinMax = maxVal - minVal;
            double kCoef = minVal / deltaMinMax;

            int dim = this[0].Position.Length;
            double sumM = 0;
            foreach (Planet p in this)
            {
                p.Mass = (p.Value - minVal) / deltaMinMax;
                sumM = sumM + p.Mass;
                p.Force = Vector.Zero(dim);
            }

            if (externalSolution != null)
            {
                externalSolutionMass = (externalSolutionValue - minVal) / deltaMinMax;
                sumM = sumM + externalSolutionMass;
                externalSolutionMass = externalSolutionMass / sumM;
            }

            //normalize mass
            foreach (Planet p in this)
                p.Mass = 0.001 + p.Mass / sumM;

            G = 100 * Math.Exp(-20 * iteration / 2000);
            iteration = iteration + 1;
            //sumarize forces
            for (int i = 0; i < this.Count - 1; i++)
            {
                Planet planetI = this[i] as Planet;
                for (int j = i + 1; j < this.Count; j++)
                {
                    Planet planetJ = this[j] as Planet;
                    double distance2 = (planetI.Position - planetJ.Position).Abs();
                    //distance2 = distance2 * distance2;

                    Vector ForceIJ = G * (planetJ.Mass * planetI.Mass / (distance2 + 1e-6)) * (planetI.Position - planetJ.Position);
                    planetJ.Force = planetJ.Force + rnd.NextDouble() * ForceIJ;
                    //planetI.Force = planetI.Force - rnd.NextDouble() * ForceIJ;
                }

                if (externalSolution != null)
                {
                    Vector distanceEVector = planetI.Position - externalSolution;
                    double distanceE2 = distanceEVector.Abs();
                    Vector ForceIE = (externalSolutionMass * planetI.Mass / (distanceE2 + double.MinValue)) * distanceEVector;
                    planetI.Force = planetI.Force - rnd.NextDouble() * ForceIE;
                }
            }


            //move
            for (int i = 0; i < this.Count; i++)
            {
                Planet planetI = this[i] as Planet;
                planetI.Position = planetI.Position + 0.1 * planetI.Velocity;
                Vector acc = (1 / planetI.Mass) * planetI.Force;
                planetI.Velocity = rnd.NextDouble() * planetI.Velocity + 0.1 * acc;
            }

            /*/evaluate
            for (int i = 0; i < this.Count; i++)
            {
                Planet planetI = this[i] as Planet;
                planetI.Value = Problem.Evaluate(planetI.Position);
            }
            */

            //evaluate
            System.Threading.Tasks.Parallel.ForEach(this,
                p =>
                {
                    IProblem lp = ProblemFactory();
                    p.Value = lp.Evaluate(p.Position);

                });

            UpdateBestSolution();
        }


    }
}
